/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.assets.shooterstates;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.assets.ArmSwitchPair;
import edu.wpi.first.wpilibj.templates.assets.statemachine.State;
import edu.wpi.first.wpilibj.templates.assets.statemachine.StateMachine;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
 *
 * @author Jarrod Risley
 */
public class RegurgitateState extends State {

    /*
     * Global Class Variables
     */
    protected static RegurgitateState SPIT_OUT =
            new RegurgitateState("Spit Out Frizbees");
    
    private static double TIMEOUT_TIME = 2;
    
    private double startTime;
    
    
    // CONSTRUCTOR //
    
    
    private RegurgitateState(String name) {
        super(name);
    } // end RegurgitateState
    
    protected void runFirst() {
        
        System.out.println(this + " running.");
        startTime = Timer.getFPGATimestamp();
        
    } // end runFirst

    protected void run() {
        
        CommandBase.shooter.reverseIndexer();
        CommandBase.intake.intake(RobotMap.REVERSE_INTAKE);
        
        if (Timer.getFPGATimestamp() > startTime + 0.5)
            MoveUpOne.MOVE_UP.resetTimesRun();
        
        if (StateMachine.fallingEdge) {
           CommandBase.shooter.decrementShooterCount();
        } // end if
        
        if (StateMachine.fallingEdgeLower) 
            CommandBase.shooter.decrementDiscCount();
        
        if (isTimedOut()
                && !CommandBase.oi.getCoDriverController().getButtonLB())
            setFinished();
        
    } // end run

    protected void runLast() {
    } // end runLast

    protected boolean isFinished() {
        return getFinished();
    } // end isFinished

    protected void end() {
        
        CommandBase.shooter.stop();
        CommandBase.intake.intake(RobotMap.STOP_INTAKE);
        
    } // end end

    protected State getChainedInstance(State nextState) {
        
        SPIT_OUT.setNext(nextState);
        return SPIT_OUT;
        
    } // end getChainedInstance
    
} // end RegurgitateState
